//////////////////////////////
// serial buffer loading

const unsigned long _timeoutPing = 15000, _timeoutConnection = 10000;
unsigned long _timeLastCommand = 0;
boolean _bPingSend = false;

const int _iMaxSerialBuffer = 1024;
const int _iMaxSerialHash = 8;
int _iSerialHashLen=0, _iSerialBufferLen=0;
char _SerialBuffer[_iMaxSerialBuffer];
char _SerialBufferHash[_iMaxSerialHash];
int _iReadMode = 0;
boolean readSerialString (String &cmd) {
  int sb;

  if(Serial1.available() == 0 && g_isConnected && _timeLastCommand>0)
  {
    unsigned long timeout = g_time - _timeLastCommand;
    if(_bPingSend && timeout>=(_timeoutPing+_timeoutConnection))
    {
      g_isConnected = false;
      _bPingSend = false;
      if(Serial)
      Serial.println("Connection still not response : disconnecting :(");
      digitalWrite(DEL_CONNECT, LOW);  
      set_pack_position();
    }
    else if(!_bPingSend && timeout>=_timeoutPing)
    {
      send_ping();
      _bPingSend = true;
      Serial.println("Connection seem to be unactive ! send a ping...");
    }
  }

  while (Serial1.available())
  {
    _timeLastCommand = g_time;

    sb = Serial1.read();
    if(sb=='*') // header/hash ?
    {
      _iReadMode=1;
      _iSerialHashLen=0;
      continue;
    }
    else if(sb=='#') // start cmd ?
    {
      if(_iReadMode!=1)
      {
        _iReadMode=0;
        continue;
      }

      _iReadMode=2;
      _SerialBufferHash[_iSerialHashLen] = '\0';
      _iSerialBufferLen=0;
      continue;
    }
    else if(sb=='$') // end cmd ?
    {
      if(_iReadMode!=2)
      {
        _iReadMode=0; 
        continue;
      }
      
      _iReadMode=0;
      
      // verify hash !
      String sCmdHash = _SerialBufferHash;
      byte iCmdHash = sCmdHash.toInt();

      byte iHash=0;
      for(int t=0; t<_iSerialBufferLen; t++) iHash += (byte)(_SerialBuffer[t]);
      if(iHash == iCmdHash)
      {
        _SerialBuffer[_iSerialBufferLen] = '\0';
        cmd = _SerialBuffer;
        return true;
      }

      Serial.println("bad hash !");
      return false;
    }

    if(_iReadMode==1)
    {
      if(_iSerialHashLen>=_iMaxSerialHash)
      {
        _iReadMode = 0;
        Serial.println("too long hash !");
        continue;
      }

      _SerialBufferHash[_iSerialHashLen]=sb;
      _iSerialHashLen++;
    }
    else if(_iReadMode==2)
    {
      if(_iSerialBufferLen>=_iMaxSerialBuffer)
      {
        _iReadMode = 0;
        Serial.println("too long request !");
        continue;
      }

      _SerialBuffer[_iSerialBufferLen] = sb;
      _iSerialBufferLen++;
    }
  }

  return false;
}

///////////////////////////////
// check commands sended
int load_int(String &s, const int start, int &iRet)
{
  int iLen = start+1;
  String num="";

  if (s[start] == '-') num += "-";
  else if(s[start] == '+') num += "";
  else return start;

  for(int i=1; i<6 && s.length()>(start+i); i++)
  {
    char c = s[start+i];
    if(!isDigit(c))
      break;
    num += c;
    iLen ++;
  }

  iRet = num.toInt();
  return iLen;
}
int load_int_nosigne(String &s, const int start, int &iRet)
{
  int iLen = start;
  String num="";

  if (s[start] == '-') num += "-";
  else if(s[start] == '+') num += "";

  for(int i=0; i<6 && s.length()>(start+i); i++)
  {
    char c = s[start+i];
    if(!isDigit(c))
      break;
    num += c;
    iLen ++;
  }

  iRet = num.toInt();
  return iLen;
}

boolean check_commands(void)
{
  String line="";
  boolean bReturn = false;
  while(readSerialString(line))
  {
    if(line.length()==0) continue;

    if(g_isDebug) Serial.println(line);
    // debug only : send_message(line);

    switch(line[0])
    {
    case 'c': // (c)onnect
      {
        // #c$
        Serial.println("Connection...");
        g_isConnected = true;
        _bPingSend = false;
        send_ready(g_isReady);
        digitalWrite(DEL_CONNECT, HIGH);  
      }
      break;
    case 'a': // (a)live
      {
        // #a$
        Serial.println("Connection still alive");
        g_isConnected = true;
        _bPingSend = false;
      }
      break;
    case 'q': // (q)uit
      {
        // #a$
        Serial.println("ask for disconnection");
        g_isConnected = false;
        _bPingSend = false;
        set_pack_position();
        digitalWrite(DEL_CONNECT, LOW);  
      }
      break;
    case 'v': // (V)ersion
      {
        // #v$
        send_version();
      }
      break;
    case 's': // (S)ervo
      {
        // #s+0+90$
        int servo=0;
        int angle=0;
        int next = 1;
        next = load_int(line, next, servo);
        next = load_int(line, next, angle);
        set_servo_position(servo, angle);
      }
      break;
    case 'p': // (P)ositions
      {
        //Serial.println(line);

        // #p+1+0+90+1+92$
        int count=0;
        int servo=0;
        int angle=0;
        int max_dep=0, dep=0;
        int next = 1;
        next = load_int(line, next, count);
        for(int c=0; c<count; c++)
        {
          next = load_int(line, next, servo);
          next = load_int(line, next, angle);
          dep = set_servo_position(servo, angle);
          if(dep>max_dep)
            max_dep = dep;
        }

        //if(max_dep>0) delay(max_dep*1.5); // wait end of servo placement ;)
      }
      break;
    default:
      {
        String sError = "invalid cmd '" + line + "'";
        send_error(sError);
        Serial.println(sError);
      }
      break;
    }
    bReturn = true;
  }
  return bReturn;
}




